﻿using DroneLib.Communication;
using System;
using System.Collections.Generic;
using System.Globalization;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using static MAVLink;

namespace DroneLib.Vehicles
{
    public class MultiRotor : Plane
    {
        readonly Dictionary<COPTER_MODE, string> _ModeStrings = new Dictionary<COPTER_MODE, string>()
        {
            {COPTER_MODE.STABILIZE,"增稳" },
            {COPTER_MODE.ACRO,"特技" },
            {COPTER_MODE.ALT_HOLD,"定高" },
            {COPTER_MODE.AUTO,"自动" },
            {COPTER_MODE.GUIDED,"引导" },
            {COPTER_MODE.LOITER,"留待" },
            {COPTER_MODE.RTL,"返航" },
            {COPTER_MODE.CIRCLE,"环绕" },
            {COPTER_MODE.LAND,"着陆" },
            {COPTER_MODE.DRIFT,"漂移" },
            {COPTER_MODE.SPORT,"运动" },
            {COPTER_MODE.FLIP,"翻滚" },
            {COPTER_MODE.AUTOTUNE,"自动调参" },
            {COPTER_MODE.POSHOLD,"定点" },
            {COPTER_MODE.BRAKE,"刹车" },
            {COPTER_MODE.THROW,"抛飞" },
            {COPTER_MODE.AVOID_ADSB,"AVOID ADSB" },
            {COPTER_MODE.GUIDED_NOGPS,"无 GPS 引导" },
            {COPTER_MODE.SMART_RTL,"智能返航" },
            {COPTER_MODE.FLOWHOLD,"光流定点" },
            {COPTER_MODE.FOLLOW,"跟随" },
            {COPTER_MODE.ZIGZAG,"ZIGZAG" },
            {COPTER_MODE.SYSTEMID,"SYSTEMID" },
            {COPTER_MODE.AUTOROTATE,"AUTOROTATE" },
            {COPTER_MODE.AUTO_RTL,"AUTO RTL" },
        };



        public override bool IsRtlMode => _ModeCode == (int)COPTER_MODE.RTL;
        public override bool IsAutoMode => _ModeCode == (int)COPTER_MODE.AUTO;
        public override bool IsAutoTuneMode => _ModeCode == (int)COPTER_MODE.AUTOTUNE;
        public override bool IsManualMode => _ModeCode == (int)COPTER_MODE.LOITER;
        public MultiRotor(byte sysId, MavStream link)
            : base(sysId, link)
        {
        }

        internal MultiRotor(byte sysId, MavStream link, MAV_TYPE type)
           : base(sysId, link)
        {
            Type = type;
        }

        public override string GetModeName(UInt32 modeCode)
        {
            if (_ModeStrings.ContainsKey((COPTER_MODE)modeCode))
                return _ModeStrings[(COPTER_MODE)modeCode];
            else
                return modeCode.ToString();
        }

        public override string[] GetModeNames()
        {
            return _ModeStrings.Values.ToArray();
        }

        protected override uint TranslateModeString(string mode)
        {
            foreach (var key in _ModeStrings.Keys)
            {
                if (_ModeStrings[key].Equals(mode) || key.ToString().Equals(mode))
                    return (uint)key;
            }

            return uint.MaxValue;
        }

        public override async Task< bool> SetMode(string modeName)
        {
            UInt32 modeCode = TranslateModeString(modeName);
            bool b = await base.SetMode(modeCode);
            if (b)
                _ModeCode = modeCode;
            return b;
        }

        public override async Task<bool> ReturnToLaunch()
        {
            return await SetMode((UInt32)COPTER_MODE.RTL);
        }

        public override async Task<bool> Auto()
        {
            //return await SetMode((UInt32)COPTER_MODE.AUTO, true);
            var result = await WaitMavCmdResult(MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, MAV_CMD.MISSION_START, 3000, 0, 9999);
            return result.Equals(MAV_RESULT.ACCEPTED);
        }

        public override async Task<bool> Land()
        {
            return await SetMode((UInt32)COPTER_MODE.LAND);
        }

        public override async Task<bool> Loiter()
        {
            return await SetMode((UInt32)COPTER_MODE.LOITER);
        }

        public override async Task< bool> AutoTune()
        {
            return await SetMode((UInt32)COPTER_MODE.AUTOTUNE);
        }

        public override async Task<bool> GotoLatLng(float lat, float lng, float alt, bool ignoreYaw, MAV_FRAME frame)
        {
            if (this.ModeCode != (UInt32)COPTER_MODE.GUIDED)
            {
                bool b = await SetMode((UInt32)COPTER_MODE.GUIDED);
                if (b == false)
                    return false;
            }

            UInt16 typeMask = UInt16.MaxValue;
            ushort MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE = ((1 << 0) | (1 << 1) | (1 << 2));
            ushort MAVLINK_SET_POS_TYPE_MASK_ALT_IGNORE = ((0 << 0) | (0 << 1) | (1 << 2));
            ushort MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE = ((1 << 3) | (1 << 4) | (1 << 5));
            ushort MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE = ((1 << 6) | (1 << 7) | (1 << 8));
            ushort MAVLINK_SET_POS_TYPE_MASK_FORCE = ((1 << 9));
            ushort MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE = ((1 << 10) | (1 << 11));

            //typeMask -= MAVLINK_SET_POS_TYPE_MASK_FORCE;

            if (lat != 0 && lng != 0)
                typeMask -= MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
            if (lat == 0 && lng == 0)
                typeMask -= MAVLINK_SET_POS_TYPE_MASK_ALT_IGNORE;

            if (ignoreYaw)
                typeMask -= MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;


            switch (frame)
            {
                case MAV_FRAME.GLOBAL_RELATIVE_ALT_INT:
                    {
                        mavlink_set_position_target_global_int_t msg = new mavlink_set_position_target_global_int_t()
                        {
                            target_system = this.SysId,
                            target_component = (byte)MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1,
                            alt = alt,
                            lat_int = (int)(lat * 1e7),
                            lon_int = (int)(lng * 1e7),
                            coordinate_frame = (byte)frame,
                            vx = (float)0,
                            vy = (float)0,
                            vz = (float)0,
                            yaw = (float)0,
                            yaw_rate = (float)0
                        };

                        msg.type_mask = typeMask;

                        SendMessage(MAVLINK_MSG_ID.SET_POSITION_TARGET_GLOBAL_INT, msg);

                        break;
                    }
                case MAV_FRAME.GLOBAL_INT:
                    {
                        mavlink_set_position_target_global_int_t msg = new mavlink_set_position_target_global_int_t()
                        {
                            target_system = this.SysId,
                            target_component = (byte)MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1,
                            alt = alt,
                            lat_int = (int)(lat),
                            lon_int = (int)(lng),
                            coordinate_frame = (byte)frame,
                            vx = (float)0,
                            vy = (float)0,
                            vz = (float)0,
                            yaw = (float)0,
                            yaw_rate = (float)0
                        };

                        msg.type_mask = typeMask;

                        SendMessage(MAVLINK_MSG_ID.SET_POSITION_TARGET_GLOBAL_INT, msg);

                        break;
                    }
                case MAV_FRAME.BODY_FRD:
                case MAV_FRAME.BODY_NED:
                    {
                        mavlink_set_position_target_local_ned_t msg = new mavlink_set_position_target_local_ned_t()
                        {
                            x = lat,
                            y = lng,
                            z = 0 - alt,
                            vx = 0,
                            vy = 0,
                            vz = 0,
                            afx = 0,
                            afy = 0,
                            afz = 0,
                            yaw = 0,
                            yaw_rate = 0,
                            type_mask = typeMask,
                            target_system = this.SysId,
                            target_component = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1,
                            coordinate_frame = (byte)frame
                        };


                        SendMessage(MAVLINK_MSG_ID.SET_POSITION_TARGET_LOCAL_NED, msg);

                        break;
                    }
            }


            return true;

        }

        public override async Task<bool> GotoXY(float x, float y, float z,bool ignoreYaw=false)
        {
            if(this.ModeCode!=(UInt32)COPTER_MODE.GUIDED)
            {
                bool b = await SetMode((UInt32)COPTER_MODE.GUIDED);
                if (b == false)
                    return false;
            }

            UInt16 typeMask;
            if (ignoreYaw)
                typeMask = 0xFFF & (~((1 << 0) | (1 << 1) | (1 << 2) | (1 << 11)));
            else
                typeMask = 0xFFF & (~((1 << 0) | (1 << 1) | (1 << 2)));

            mavlink_set_position_target_local_ned_t msg = new mavlink_set_position_target_local_ned_t(
                    0,
                    x, y, 0 - z,
                    0, 0, 0,
                    0, 0, 0, 
                    0, 0,
                    typeMask,
                    this.SysId,
                    (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1,
                    (byte)MAV_FRAME.BODY_NED
                );

            SendMessage(MAVLINK_MSG_ID.SET_POSITION_TARGET_LOCAL_NED, msg);
            return true;
        }

        public override async Task<bool> SetYaw(float yaw)
        {
            UInt16 typeMask = 0xFFFF & (~((1 << 3) | (1 << 4) | (1 << 5) | (1 << 10) | (1 << 11)));
            float value = (float)(yaw * Math.PI / 180);
            mavlink_set_position_target_local_ned_t msg = new mavlink_set_position_target_local_ned_t(
                    0,
                    0, 0, 0,
                    0, 0, 0,
                    0, 0, 0,
                    value, value*2,
                    typeMask,
                    this.SysId,
                    (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1,
                    (byte)MAV_FRAME.BODY_NED
                );

            SendMessage(MAVLINK_MSG_ID.SET_POSITION_TARGET_LOCAL_NED, msg);
            return true;
        }

        public bool SetYawGlobal(float yawDegrees)
        {
            // 将角度转换为弧度
            float yawRadians = (float)(yawDegrees * Math.PI / 180.0);

            // 构造SET_ATTITUDE_TARGET消息
            var msg = new mavlink_set_attitude_target_t
            {
                time_boot_ms = (uint)Environment.TickCount,
                target_system = (byte)this.SysId,
                target_component = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1,
                type_mask = 0b00000111, // 掩码 - 只控制偏航
                //q = new float[] { 0, 0, 0, 0 }, // 忽略四元数
                body_roll_rate = 0,
                body_pitch_rate = 0,
                body_yaw_rate = yawRadians, // 目标偏航角度（弧度）
                thrust = 0,
                q=new float[] {0,0,0,0},
                thrust_body=new float[] {0,0,0,0},
            };
            SendMessage(MAVLINK_MSG_ID.SET_ATTITUDE_TARGET, msg);

            return true;
        }
    }
}
